/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/bezier_traj.h
 * @Description  : BezierTrajectory类的接口定义，表示基于Bezier曲线的轨迹生成和管理。 
 *                 该类提供了创建轨迹、获取起始和结束角度、计算曲率、生成速度等功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:44:08
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include "trajectory.h"
#include "bezier.h"
#include "pso_method.h"

// #define BEZIER_RELOAD_AREA_THRESHOLD	0.05
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "CBezierTraj".
class BezierTrajectory : public Trajectory
{
 public:
    // Default constructor
    BezierTrajectory();

    // Destructor
    ~BezierTrajectory() override;

    // Vehicle's drive mode on the trajectory
    short GetDrivePattern() const override;

    // Get the vehicle's start steer angle
    Angle& StartSteerAngle() override;

    // Get the vehicle's end steer angle
    Angle& EndSteerAngle() override;

    // Get the vehicle's start heading angle
    Angle& StartHeading() override;

    // Get the vehicle's end heading angle
    Angle& EndHeading() override;

    // Get the start point of the trajectory
    Point StartPoint() override;

    // Get the end point of the trajectory
    Point EndPoint() override;

    // Set the progress variable to specify the current point
    void SetRelProgress(float rate, float progress) override;

    // The velocity generation function
    Velocity &VelocityFun() override;

    // The steer velocity generation function
    Angle &SteeringFun() override;

    // The curvature at the start point
    float StartCurvature() override;

    // The curvature at the end point
    float EndCurvature() override;

    // Get the deviation between the trajectory and the specified posture
    bool ProjectPosture(Posture& posture, ProjectData* data, bool init_posture = false) override;

    // Get the profile value for the given velocity vector
    // virtual float GetProfileArea(float fLinearArea) override;

    // Get the profile value for the given velocity vector
    float GetProfileValue(float linear_velocity) override;

    // Get the profile slope for the given acceleration
    float GetProfileSlope(float acc) override;

    // Get the linear according to the profile value
    float GetLinearVel(float value) override;

    Velocity VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) override;

    // 车头角度减去轨迹切线角度(用来纠偏)
    Angle HeadToTangent() override;

 public:
    // Create the Bezier trajectory.
    void CreateTrajectory(Posture& start_posture, Posture& end_posture, float len1, float len2, MoveDir wheel_move_dir,
                        bool is_shift, Angle& shift_heading, float from = 0, float to = -1.0f);

    // Create the Bezier trajectory.
    void CreateTrajectory(Point& start_point, Point& ctrl_point1, Point& ctrl_point2, Point& end_point, MoveDir wheel_move_dir,
                        bool is_shift, Angle& shift_heading, float from = 0, float to = -1.0f);

    // Get the Bezier curve object
    Bezier* GetBezierObject();

    // Get the first control point of the trajectory
    Point FirstControlPoint();

    // Get the second control point of the trajectory
    Point SecondControlPoint();

    // The move is a shift
    bool IsShiftMove() const;

    // Get the radius of the arc to be replaced
    float Radius();

    // Get the current radius
    float CurrentRadius();

    // Get the curve length
    float GetCurveLength();

    // Get the total length of the trajectory
    float GetTotalLength();

    //  Get the current progress
    float GetCurT();

    //  Get the last progress
    float GetLastT();

    //  Get the difference between the current progress and the last progress
    float GetDeltaT();

    //  set Reload area flag
    void SetReloadAreaFlag(bool flag);
 private:
    // Do basic initializations
    void Init(MoveDir wheel_move_dir, bool is_shift, Angle& shift_heading, float from, float to);

    // Do initializations that are specific to the Bezier trajectory
    void CreateBezier(Posture& start_posture, Posture& end_posture, float len1, float len2);

    // Do initializations that are specific to the Bezier trajectory
    void CreateBezier(const Point& start_point, const Point& ctrl_point1, const Point& ctrl_point2, const Point& end_point);

    // Set the progress to specify the current absolute point
    void SetCurrentT(float rate, float cur_t, bool update = true, bool updateRate = true);

    // add by yxj
    bool CanSteerReserve(Angle steer_angle);

    // set the wheel direction
    void SetWheelDir();
 private:
    Bezier* bezier_;           // Pointer to the Bezier curve object
    Angle start_heading_;      // Tangent angle at the start point
    Angle end_heading_;        // Tangent angle at the end point
    float start_curvature_;    // Curvature at the start point
    float end_curvature_;      // Curvature at the end point

    Angle shift_heading_;      // Vehicle body translation direction angle
    bool is_shift_move_;       // Whether it is omnidirectional movement
    MoveDir side_move_dir_;    // 只是记录平移方向；

    Angle start_steer_;        // start steer angle;Temp the are not used
    Angle middle_steer_;       // middle steer angle;
    Angle end_steer_;          // end steer angle
    // BOOL m_bForceReverse;   // force steer reverse
    bool is_steer_reverse_;    // whether steer reverse

    float last_plan_t_;        // last plan T(value:0~1)
    float plan_delta_t_;       // plan delta T(currentT - lastT)
    float last_ref_t_;         // last reference T
    float ref_delta_t_;        // reference delta T(currentT - lastT)
    Posture last_posture_;     // 上一次位置;上一次位置和当前位置不变时; 直接使用上次计算的偏差
    ProjectData last_project_data_; // 上一次偏差记录 上一次位置和当前位置不变时; 直接使用上次计算的偏差 m_LastProjectData
    bool reload_area_flag_;         // important: whether it can be modify current progress(m) by current actual posture
};
